#include "Defines.inc"

Global Integer Status
Global Integer Status_temp

Function Main
 	Call RobotInit
 	Call SelectProgram
Fend
Function RobotInit
	Cls
	Reset
	Motor On
	Speed In(SPEED_GLOBAL)
	Accel 100, 100
Fend


Function SelectProgram
	Do
	Status = In(STATUS_IN)
	If Sw(POWER_MODE) Then
		Power High
	Else
		Power Low
	EndIf
	If Status_temp = Status Then
		GoTo Ext99
	EndIf
	Status_temp = Status
	Select Status
		Print "#Status", Status
		Case 99
			Call GoHome
		Case 1
			Call Pick
		Case 2
			Call TakePhoto1
		Case 3
			Call TakePhoto2
		Case 4
			Call TakePhoto3
		Case 5
			Call TakePhoto4
		Case 6
			Call TakePhoto5
		Case 7
			Call TakePhoto6
		Case 8
			Call PreTakePhoto
		Case 9
			Call PreDummy
		Case 10
			Call DropNG1
		Case 11
			Call DropNG2 'b
		Case 12
			Call DropOK_Ready 'c
		Case 13
			Call DropOK 'd
		Case 14
			Call Dummy1 'e
		Case 15
			Call Dummy2 'f
		Case 16
			Call Dummy3 'g
		Case 17
			Call Dummy4 'h
		Case 18
			Call Dummy5 'i
		Case 19
			Call Dummy6 'j
		Case 20
			Call Dummy_Loader 'k
		Case 21
			Call Dummy1_Back 'L
		Case 22
			Call Dummy2_Back 'M
		Case 23
			Call Dummy3_Back 'N
		Case 24
			Call Dummy4_Back 'O
		Case 25
			Call Dummy5_Back 'P
		Case 26
			Call Dummy6_Back 'Q
	Send
	Speed In(SPEED_GLOBAL)
	Print "STATUS_OUT"
	Out STATUS_OUT, Status
Ext99:
	Loop
Fend
Function GoHome
	Routine(GoSafePoint, 3)

Fend
Function Pick
	Routine(GoSafePoint, 3)
	Go PT_PICK

Fend


Function TakePhoto1
 	Call CamRoutine
 	Real ub
 	ub = CU(PTTakePhoto1)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
	Go PTTakePhoto1
	
	'TcpSendData("2")
Fend
Function TakePhoto2
	Call CamRoutine
 	Real ub
 	ub = CU(PTTakePhoto2)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
	Go PTTakePhoto2
	'TcpSendData("3")
Fend
Function TakePhoto3
    Call CamRoutine
    Real ub
 	ub = CU(PTTakePhoto3)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
	Go PTTakePhoto3
'	TcpSendData("4")
Fend
Function TakePhoto4
	Call CamRoutine
	Real ub
 	ub = CU(PTTakePhoto4)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
	Go PTTakePhoto4
'	TcpSendData("5")
Fend
Function TakePhoto5
	Call CamRoutine
	Real ub
 	ub = CU(PTTakePhoto5)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
	Go PTTakePhoto5
'	TcpSendData("6")
Fend
Function TakePhoto6
	Call CamRoutine
	Real ub
 	ub = CU(PTTakePhoto6)
 	P600 = CurPos
	CU(P600) = ub
	Go P600
    Go PTTakePhoto6
'	TcpSendData("7")
Fend
Function PreTakePhoto
	Call CamRoutine
'	TcpSendData("8")
Fend

Function DropNG1

	If (GoSafePoint < 2) Then
	Routine(GoSafePoint, 2)
	EndIf
	Go PTDropNG1
'	TcpSendData("A")
Fend
Function DropNG2
	If (GoSafePoint < 2) Then
	Routine(GoSafePoint, 2)
	EndIf
	Go PTDropNG2
'	TcpSendData("B")
Fend
Function DropOK_Ready
	Integer J
	J = GetCurrentPointID
	If J <> DropOK_ID And J <> DropOK_Ready_ID Then
		Routine(GoSafePoint, 0)
	EndIf
	Go PTDropOK +Z(30)
'	TcpSendData("C")
Fend
Function DropOK
	Integer J
	J = GetCurrentPointID
	If j <> DropOK_ID And j <> DropOK_Ready_ID Then
		Routine(GoSafePoint, 0)
	EndIf
	Go PTDropOK +Z(30)
	Go PTDropOK
'	TcpSendData("D")
Fend
Function PreDummy
	DummyRoutine(GoSafePoint, 4)
Fend
Function Dummy1
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy1 :Z(0)
	Go PTDummy1
'	TcpSendData("E")
Fend
Function Dummy2
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy2 :Z(0)
	Go PTDummy2
'	TcpSendData("F")
Fend
Function Dummy3
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy3 :Z(0)
	Go PTDummy3
'	TcpSendData("G")
Fend
Function Dummy4
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy4 :Z(0)
	Go PTDummy4
'	TcpSendData("H")
Fend
Function Dummy5
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy5 :Z(0)
	Go PTDummy5
'	TcpSendData("I")
Fend
Function Dummy6
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy6 :Z(0)
	Go PTDummy6
'	TcpSendData("J")
Fend
Function Dummy_Loader
	DummyRoutine(GoSafePoint, 4)
	Go CAL61 :Z(0)
	Go CAL61
	Speed In(SPEED_INSERT)
	Go PTDummy_Loader
'	TcpSendData("K")
Fend
Function Dummy1_Back
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy1 +Y(-D_OUT) :Z(0)
    Go PTDummy1 +Y(-D_OUT)
   	Speed In(SPEED_INSERT)
	Go PTDummy1
'	TcpSendData("L")
Fend
Function Dummy2_Back
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy2 +Y(-D_OUT) :Z(0)
    Go PTDummy2 +Y(-D_OUT)
    Speed In(SPEED_INSERT)
	Go PTDummy2
'	TcpSendData("M")
Fend
Function Dummy3_Back
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy3 +Y(-D_OUT) :Z(0)
    Go PTDummy3 +Y(-D_OUT)
    Speed In(SPEED_INSERT)
	Go PTDummy3
'	TcpSendData("N")
Fend
Function Dummy4_Back
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy4 +Y(-D_OUT) :Z(0)
    Go PTDummy4 +Y(-D_OUT)
    Speed In(SPEED_INSERT)
	Go PTDummy4
'	TcpSendData("O")
Fend
Function Dummy5_Back
	DummyRoutine(GoSafePoint, 4)
	Go PTDummy5 +Y(-D_OUT) :Z(0)
    Go PTDummy5 +Y(-D_OUT)
    Speed In(SPEED_INSERT)
	Go PTDummy5
'	TcpSendData("P")
Fend
Function Dummy6_Back
    DummyRoutine(GoSafePoint, 4)
	Go PTDummy6 +Y(-D_OUT) :Z(0)
    Go PTDummy6 +Y(-D_OUT)
    Speed In(SPEED_INSERT)
	Go PTDummy6
'	TcpSendData("Q")
Fend

Function Routine(source As Integer, dest As Integer)
	P(200) = RUN0
	P(201) = RUN1
	P(202) = RUN2
	P(203) = RUN3
	P(204) = RUN3
	P(205) = RUN5
	Integer i
	If source < dest Then
	For i = source To dest
			Pass P(200 + i)
	Next
	Else
	For i = source To dest Step -1
			Pass P(200 + i)
	Next
	EndIf
Fend

Function DummyRoutine(source As Integer, dest As Integer)
	P(200) = RUN0
	P(201) = RUN1
	P(202) = RUN2
	P(203) = RUN3
	P(204) = RUN4
	P(205) = RUN5
	Integer i
	If source < dest Then
	For i = source To dest
			Pass P(200 + i)
	Next
	Else
	For i = source To dest Step -1
            Pass P(200 + i)
	Next
	EndIf
Fend

Function CamRoutine
	Integer id
	Real VU
	id = GetCurrentPointID
	If IsCameraID(id) <> True Then
		Routine(GoSafePoint, 1)
    EndIf
Fend

Function IsCameraID(id As Integer) As Boolean
	If id = Takephoto1_ID Or id = Takephoto2_ID Or id = Takephoto3_ID Or id = Takephoto4_ID Or id = Takephoto5_ID Or id = Takephoto6_ID Or id = Takephoto7_ID Or id = Takephoto8_ID Or id = Takephoto_TEMP_ID Then
		IsCameraID = True
	Else
		IsCameraID = False
    EndIf
Fend

Function IsDummyID(id As Integer) As Boolean
	If id = Dummy1_ID Or id = Dummy2_ID Or id = Dummy3_ID Or id = Dummy4_ID Or id = Dummy5_ID Or id = Dummy6_ID Then
		IsDummyID = True
	Else
		IsDummyID = False
    EndIf
Fend

Function GetCurrentPointID As Integer
	Integer i, j
	Real f1
	Real f2
	f2 = Dist(P(0), CurPos)
	For i = 1 To MAX_ID
		f1 = Dist(P(i), CurPos)
		If f1 < f2 Then
			j = i
			f2 = f1
		EndIf
	Next
'	Go P(j)
	GetCurrentPointID = j
Fend

Function GoSafePoint() As Integer
	Integer j, k
	j = GetCurrentPointID
	If J = DropOK_ID Or J = DropOK_Ready_ID Then   'oktray
		Go PTDropOK +Z(30)
		Go RUN0
		k = 0
	ElseIf J = GET_ID Then
	    Go P(J) +Y(-D_OUT)
        Go P(J) +Y(-D_OUT) :Z(0)
		k = 3
	ElseIf IsDummyID(J) Then       'quliao
        Go P(J) +Y(-D_OUT)
        Go P(J) +Y(-D_OUT) :Z(0)
		Go RUN4
		k = 4
	ElseIf J = Dummy_Loader_ID Or J = CAL61_ID Then
		Go CAL61
		Go CAL61 :Z(0)
		Go RUN4
		k = 4
	ElseIf J = CAL62_ID Then
		Go RUN4
		k = 4
    ElseIf IsCameraID(J) Then   'paizhao
		Go RUN1
		k = 1
	ElseIf J = DropNG1_ID Or J = DropNG2_ID Then   'ngtray
		Go RUN5
		k = 5
	ElseIf J = RUN0_ID Then
		k = 0
	ElseIf J = RUN1_ID Then
		k = 1
	ElseIf J = RUN2_ID Then
		k = 2
	ElseIf J = RUN3_ID Then
		k = 3
	ElseIf J = RUN4_ID Then
		k = 4
	ElseIf J = RUN5_ID Then
		k = 5
	EndIf
    GoSafePoint = k
Fend


Function test
	Do
	Out 70, &Hff
	Integer var1
	var1 = In(70)
	Wait (500)
	Loop
Fend

